package org.checkboo.math;


public class Quaternion {
	
	private float w;
	private float x;
	private float y;
	private float z;

	public Quaternion(){
		w = x = y = z = 0;
	}
	
	public Quaternion(float w, float x, float y, float z){
		this.w = w;
		this.x = x;
		this.y = y;
		this.z = z;
	}
	
	public Quaternion(Quaternion quat){
		this.w = quat.w;
		this.x = quat.x;
		this.y = quat.y;
		this.z = quat.z;
	}
	
	public Quaternion set(float w, float x, float y, float z){
		this.w = w;
		this.x = x;
		this.y = y;
		this.z = z;
		
		return this;
	}
	
	public Quaternion set(Quaternion quat){
		this.w = quat.w;
		this.x = quat.x;
		this.y = quat.y;
		this.z = quat.z;
		
		return this;
	}
	
	public Quaternion setIdentity(){
		this.w = 1;
		this.x = this.y = this.z = 0;
		
		return this;
	}
	
	public Quaternion setZero(){
		this.w = this.x = this.y = this.z = 0;
		
		return this;
	}
	
	public Quaternion add(Quaternion second){
		this.w += second.w;
		this.x += second.x;
		this.y += second.y;
		this.z += second.z;
		
		return this;
	}
	
	public void addRes(Quaternion second, Quaternion result){
		result.w = this.w + second.w;
		result.x = this.x + second.x;
		result.y = this.y + second.y;
		result.z = this.z + second.z;
	}
	
	public Quaternion subtract(Quaternion second){
		this.w -= second.w;
		this.x -= second.x;
		this.y -= second.y;
		this.z -= second.z;
		
		return this;
	}
	
	public void subtractRes(Quaternion second, Quaternion result){
		result.w = this.w - second.w;
		result.x = this.x - second.x;
		result.y = this.y - second.y;
		result.z = this.z - second.z;
	}
	
	public Quaternion multiply(Quaternion second){
		Quaternion tmp = new Quaternion(this);
		
		this.w = tmp.w*second.w - tmp.x*second.x - tmp.y*second.y - tmp.z*second.z;
		this.x = tmp.w*second.x + tmp.x*second.w + tmp.y*second.z - tmp.z*second.y;
		this.y = tmp.w*second.y - tmp.x*second.z + tmp.y*second.w + tmp.z*second.x;
		this.z = tmp.w*second.z + tmp.x*second.y - tmp.y*second.x + tmp.z*second.w;
		
		return this;
	}
	
	public void multiplyRes(Quaternion second, Quaternion result){
		result.w = this.w*second.w - this.x*second.x - this.y*second.y - this.z*second.z;
		result.x = this.w*second.x + this.x*second.w + this.y*second.z - this.z*second.y;
		result.y = this.w*second.y - this.x*second.z + this.y*second.w + this.z*second.x;
		result.z = this.w*second.z + this.x*second.y - this.y*second.x + this.z*second.w;
	}
	
	public Quaternion divide(float scalar){
		this.w /= scalar;
		this.x /= scalar;
		this.y /= scalar;
		this.z /= scalar;
		
		return this;
	}
	
	public Quaternion conjugate(){
		this.x = -this.x;
		this.y = -this.y;
		this.z = -this.z;
		
		return this;
	}
	
	public float norm(){
		float result = FloatMath.sqr(this.w) + FloatMath.sqr(this.x) + FloatMath.sqr(this.y) + FloatMath.sqr(this.z);
		
		return result;
	}
	
	public void normalize(){
		divide(FloatMath.sqrt(w*w + x*x + y*y + z*z));
	}
	
	public Quaternion inverse(){
		float norm = norm();
		conjugate().divide(norm);
		
		return this;
	}
	
	public float selection(){
		return this.w;
	}
	
	public Quaternion fromAngleAxis(float angle, Vector3f axis){
		float rads = FloatMath.degToRad(angle);
		axis.normalize();
		this.w = FloatMath.cos(rads/2);
		this.x = axis.getX() * FloatMath.sin(rads/2);
		this.y = axis.getY() * FloatMath.sin(rads/2);
		this.z = axis.getZ() * FloatMath.sin(rads/2);
		
		return this;
	}
	
	public void toAngleAxis(float angleResult, Vector3f axisResult){
		if(Math.abs(this.w) == 1){
			angleResult = 0.0f;
			axisResult.zero();
		}
		else{
			angleResult = FloatMath.radToDeg(2 * FloatMath.acos(this.w));
			axisResult.setX(this.x/FloatMath.sqrt(1 - FloatMath.sqr(this.w)));
			axisResult.setY(this.y/FloatMath.sqrt(1 - FloatMath.sqr(this.w)));
			axisResult.setZ(this.z/FloatMath.sqrt(1 - FloatMath.sqr(this.w)));
		}
	}
	
	public Matrix33f toRotationMatrix(){
		Matrix33f result = new Matrix33f();
		
		result.setV11(1 - 2 * FloatMath.sqr(this.y) - 2 * FloatMath.sqr(this.z));
		result.setV12(2 * this.x * this.y - 2 * this.w * this.z);
		result.setV13(2 * this.x * this.z + 2 * this.w * this.y);
		result.setV21(2 * this.x * this.y + 2 * this.w * this.z);
		result.setV22(1 - 2 * FloatMath.sqr(this.x) - 2 * FloatMath.sqr(this.z));
		result.setV23(2 * this.y * this.z - 2 * this.w * this.x);
		result.setV31(2 * this.x * this.z - 2 * this.w * this.y);
		result.setV32(2 * this.y * this.z + 2 * this.w * this.x);
		result.setV33(1 - 2 * FloatMath.sqr(this.x) - 2 * FloatMath.sqr(this.y));
		
		return result;
	}
	
	public Quaternion fromRotationMatrix(Matrix33f matrix){
		if(matrix.trace() > 0){
			this.w = FloatMath.sqrt(matrix.trace() + 1)/2;
			this.x = (matrix.getV23()-matrix.getV32())/(4*this.w);
			this.y = (matrix.getV31()-matrix.getV13())/(4*this.w);
			this.z = (matrix.getV12()-matrix.getV21())/(4*this.w);
		}
		else{
			if(matrix.getV11() > matrix.getV22() && matrix.getV11() > matrix.getV33()){
				this.x = FloatMath.sqrt(matrix.getV11() - matrix.getV22() - matrix.getV33() + 1)/2;
				this.w = (matrix.getV23()-matrix.getV32())/(4*this.x);
				this.y = (matrix.getV12()+matrix.getV21())/(4*this.x);
				this.z = (matrix.getV13()+matrix.getV31())/(4*this.x);
			}
			else if(matrix.getV22() > matrix.getV11() && matrix.getV22() > matrix.getV33()){
				this.y = FloatMath.sqrt(matrix.getV22() - matrix.getV11() - matrix.getV33() + 1)/2;
				this.w = (matrix.getV31()-matrix.getV13())/(4*this.y);
				this.x = (matrix.getV12()+matrix.getV21())/(4*this.y);
				this.z = (matrix.getV13()+matrix.getV31())/(4*this.y);
			}
			else{
				this.z = FloatMath.sqrt(matrix.getV33() - matrix.getV11() - matrix.getV22() + 1)/2;
				this.w = (matrix.getV12()-matrix.getV21())/(4*this.z);
				this.x = (matrix.getV13()+matrix.getV31())/(4*this.z);
				this.y = (matrix.getV23()+matrix.getV32())/(4*this.z);
			}
		}
		return this;
	}

	public float getW() {
		return w;
	}

	public void setW(float w) {
		this.w = w;
	}

	public float getX() {
		return x;
	}

	public void setX(float x) {
		this.x = x;
	}

	public float getY() {
		return y;
	}

	public void setY(float y) {
		this.y = y;
	}

	public float getZ() {
		return z;
	}

	public void setZ(float z) {
		this.z = z;
	}
	
	public String toString(){
		return this.x+"*i + "+this.y+"*j + "+this.z+"*k + "+this.w;
	}

}
